package project.robot.detection;

import java.util.ArrayList;
import java.util.Calendar;

import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.RangeFinder;

public class UltrasonicDetectionSystem {

	private RangeFinder range_finder = null;
	private float max_dist;
	private boolean detectionOn= false;
	private int delay;
	private ArrayList<UltrasonicListener> listeners = null;	
		
	public UltrasonicDetectionSystem(SensorPort rangeSensorPort, float max_range, int scanDelay)
	{
		this.range_finder = new UltrasonicSensor(rangeSensorPort);
		this.max_dist = max_range;
		this.delay = scanDelay;
		
		Thread x = new MonitorThread();
		x.setName("ULTRASONIDO");
		x.start();
	}	
	
	public void startDetection()
	{
		this.detectionOn= true;
	}
	
	public void stopDetection()
	{
		this.detectionOn= false;
	}
	 
    private float scan() {
		float range= range_finder.getRange();
		return (range < max_dist)?range:max_dist;
	}	
	
	protected void notifyListeners(float rangeReading) {
		if(listeners != null) { 
			for(UltrasonicListener l : listeners) {
				l.rangeReadingObtained(rangeReading);
			}
		}
	}
		
	public void addListener(UltrasonicListener l){
		if(listeners == null )listeners = new ArrayList<UltrasonicListener>();
		listeners.add(l);
	}
	
	private class MonitorThread extends Thread{
		@Override
		public void run() {
			while(true) {
				
				float distance = (detectionOn?scan():0);
				
				if(distance > 0) notifyListeners(distance);
				
				long tiempo = Calendar.getInstance().getTimeInMillis();
				
				while (Calendar.getInstance().getTimeInMillis()-tiempo <= delay)
					Thread.yield();
	
				/*try {
					long elapsed_time = System.currentTimeMillis() - prev_time;
					long actual_delay = delay - elapsed_time;
					if(actual_delay < 0) actual_delay = 0;
					Thread.sleep(delay);
					//prev_time = System.currentTimeMillis();
					//System.out.println("SE DESPERTO " + System.currentTimeMillis());

				} catch (InterruptedException e) {
					e.printStackTrace();
					
				}*/
			}
		}
	}
}
